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SIMULATION OF A NAVIGATOR ALGORITHM FOR. A LOW-COST GPS' RECEIVER 

by 

Ward F. Hodge 
SUMMARY 

The analytical structure of an existing navigator algorithm for a low 
cost GPS receiver is described in detail to facilitate its implementation on 
in— house digital computers and real-time simulators. The material presented 
includes a simulation of GPS pseudorange measurements, based on a two-body 
representation of the NAVSTAR spacecraft orbits and a four component model 
of the receiver bias errors. A simpler test for loss of pseudorange meas- 
urements due to spacecraft shielding is also noted. 

INTRODUCTION 

The global positioning system (GPS) is a worldwide .navigatlon.net- , . 
work, being developed by the Department of Defense, which eventually will 
comprise a constellation of 2A NAVSTAR earth satellite spacecraft for 
transmitting navigation infoimatlon to system users. Its obvious potential 
for additional utilization by a large number of non-military users has 
attracted attention in fields as diverse as general aviation (GA) and 
shipping. The cost of the GPS receiver for such users, which measures 
pseudorange from the user craft to four of the NAVSTAR spacecraft simul- 
taneously, has been recognized as one of its major design factors 
(reference 1) . The fact that simultaneous measurement of the pseudorange 
data requires a minimum of four receiving channels represents a significant 
cost consideration. 
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Interest in developing a practical low-cost GPS receiver for civilian 
users has led to the formulation of position fixing and navigation schemes 

h flSAfV-nn thp- »»Hc» af_a^ single channel instrument-i As-psaudorange dataimust: 

then be received sequentially rather than simultaneously, there is a need 
for investigating the resulting navigational accuracy and the effect of 
user craft motion between measurements. In this connection, the effects of 
intentional degradation of the GPS signals, loss of pseudorange data due to 
spacecraft shielding, and the time between updating all require additional 
study. The purpose in this paper is to describe one such low-cost navigator 
algorithm, as devised by the Mitre Corporation (ref erence 2) , in sufficient 
detail to facilitate its implementation on digital computers and real-time 
simulators at LRC. The block diagram in figure 1 depicts the overall simu- 
lation structure, which comprises two main elements that respectively de- 
fine the pseudorange measurements model and the navigator algorithm. Addi- 
tionally, the FORTRAN names of principal program variables and calls to 
subroutines are indicated at appropriate places on figure 1 for convenience 
pf reference to the current in-house version of Mitre’s programing (see 
reference 2), which is included in the APPENDIX. 
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SYMBOLS 

spacecraft azimuth, deg 

semimajor axis of spacecraft orbit, n. mi. 
equatorial radius of the geoid, n. mi. 

GPS pseudorange bias error, equivalent n. mi. 
ionospheric delay component of b, n. mi. 

multipath and receiver noise component of b,. n. mi. 

user clock bias component of b, n. mi. 

intentional degradation component of b, n. mi. 

polar radius of the geoid, n. mi. 

eccentric anomaly of spacecraft orbit, deg 
eccentricity of spacecraft orbit 
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eccentricity of the geoid 

spacecraft true anomaly, deg 
pseudorange partial derivative matrix 
local topocentric altitude, ft 
k^^ row of H matrix 

inclination of spacecraft orbit, deg 
mean anomaly of spacecraft orbit, deg 
number of At updating periods (see eqn. (4)) 
spacecraft mean motion, deg/sec (eqn. (4)) 
local topocentric position, n. mi. 
pseudorange measurements vector, n. mi. 
k^^ pseudorange measurement, n. mi. 

time since pericenter passage in spacecraft orbit, hrs 
geographic or geodetic East longitude, deg 




t tme^ t±ne, hrs^^ 

t GMT at start of simulation, hrs 

o 

U local topocentric position of user craft and pseudorange bias error, 

n. mi. (see equation 9 (a)) 

IL tisgir cra£ti velaciti^v^ectx?ir^^ Tti> nrj/hr:- 

X., Y, Z rectangular components of R, n. mi.. 

X, y, z rectangular components of p, n. mi. 
a,3 smoothing coefficients (see eqn* (11)) 

e apparent spacecraft elevation, deg 

9 geocentric right ascension, deg 

3 2 

y universal gravitational constant, (n. mi) /sec 

p geocentric distance, n. mi. 

geodetic latitude, deg 
({)* geocentric latitude, deg 

<p user craft roll or bank angle, deg 

^ user craft true velocity heading, deg 

Q right ascension of“ ascending mode of spacecraft orbit, deg 

60 argument of pericenter of spacecraft orbit, deg 

60 axial rotation rate pf the earth, 15°/hr 

Subscripts: 

CT cross track 

G Greenwich meridian 

k k^^ spacecraft 

p GPS constellation index for spacecraft phase angle 

GPS constellation index for spacecraft orbit plane phase angle 
or local reference site 


s 
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Notation: 


A ( ) incremental value or dwell time interval 

( ) estimated value 

rp 

( ) matrix transpose 

( ) ^matrix inverse 

(] vector ■ 


PSELDORANGE MEASUREMENTS MODEL 


The flow diagram in Figure 1 shows the simulation employed for the 
pseudorange measurements to incorporate models for the respective motions 
and positions of the NAVSTAR spacecraft and the user craft relative to a 
local topocentrlc reference site, and for the measurement errors associated 
with the GPS receiver. The first of these is ionospheric delay, which is 
modeled as a deterministic error in terms of an ion density scale factor and 
pseudorange path length. Multipath error and receiver noise is modeled next, 
and is represented simply as random white noise. The third type of measure- 
ment error simulated is the user clock bias, which includes random white 
noise and a starting offset in addition to drift and aging terms that increase 
with time and are associated with the assumption that_the clock is driven by a 
crystal oscillator. The remaining error source is the intentional degradation 
bias, which is generated separately for each spacecraft by independently pass- 
ing uniform random numbers through an exponential filter. Scaling of the 
filter's Gaussian output is then adjusted to give a standard deviation on the 
pseudorange for each spacecraft such that the 2o user position error result- 
ing from all four spacecraft is 500 meters. , 
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Figure 1. SIMULATION BLOCK DIAGRAM 
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GPS/NAVSTAR Spacecraft Position . - As Indicated in Figure 1, there are 
several steps in determining the required true topocentric positions of 
the four NAVSTAR spacecraft. This process begins with the inertial posi- 
tion of the spacecraft orbit relative to geocentric equatorial axes I, J, K 
having I toward the vernal equinox and K along the earth's spin axis as 
illustrated in Figure 2. The orbit orientation 
relative to these axes is defined by the 
three angles i, and w as indicated. 

Let PQW be a set of perifocal axes 
aligned with the orbit plane such that 
W coincides with the normal to the 
orbit plane, and P is along the line 
of apsides toward perigee. The 
position of the spacecraft in 
its orbital plane is given I 


Earth Spin Axis 
Q 


(Y) 


equinox 


by its geocentric 
distance p , and true 
anomaly f. The transformation 
to perifocal coordinates P, Q, W is given by 



vernal 


ascending 
node 


line of nodes 


Figure 2, Spacecraft Orbit Orientation 


P = p cos f 
Q = p sin f 
W = 0 


( 1 ) 


which are then transformed to I, J, K inertial coordinates 

xV rp 

Y I = w (n, i, (0) ^ \ Q 
z 1 / w 


( 2 ) 


where. 
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I cos 0 ) cos Jr - cos £ sfn oi sin fi | -sin w cos 0. -cos. 1 cos (o sin JI.' sin 1 sin. O-N 

cos w sin + cos i sin U) cos | —sin w sin +cos i cos w cos fii— sin i cos 

... •' ‘ 

sin w sin 1 , cos u sin i i cos i 


The; task- o£ obtaining p and f, for evaluating equations (1) and (2) for 
each of the four spacecraft, is considerably simplified by the assumption of 
two-body circular orbits (e = 0) . In this case the position of the apsidal 
line in the orbit plane and, the time of perigee passage are arbitrary, and 
U) and T may be set to zero so that the apsidal and nodal lines coincide and 
equation (2) reduces to 


X / cos - cos i sin cos f 

Y > =1 sin Q -COS i cos Kp sin f; 
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0 


sin 1 


(2a) 


and usual two-body orbital equations 


_ a(l-e^) 


1 + e cos f 


- a (1-e cos E) 


tan 


I ■ iBT 


tan E 
2 


(3) 


M 


= n(t-T) = E - e sin E 


become 


P = a . 

f = M = E = nt. 


T = 0 


-1F 


(3a) 
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The next step is to calculate the unique values of for each of 
the four spacecraft, which specify their locations in the GPS constellation. 
As the planned configuration is for eight spacecraft equally spaced in each 
of three orbit planes, inclined 63° to the equator with their nodal lines 
equally spaced 120° apart, each spacecraft will have a unique combination 
of and an in-plane angle f^. The arrangement assumed for the present 
simulation is illustrated in figure 3 for one such orbiting ring of eight 
spacecraft. In addition to S?p , each of the three spacecraft rings has an 

initial rotation f from the line of apsides as indicated. Taking f and 

P :• 

f into account, the true anomaly for any of the spacecraft is given by 
• s 


f (t) = f + f + nt . j (^) 

k P s 

in which, 

f (s-l),l<s<8 

s 4 



t = t + mAt 

o 



Figure 3, Spacecraft Orbital 
Spacing Geometry. 
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To illustrate these calculations, suppose the third spacecraft (k=3) happens 

to. be the fourth one in the first ring so that p =-l and s — 4. The true 

, . . , r /*.\ -'f/io + 3'JT// + nt, and fi, = 0. Thus, the 

anomaly xs then given by f^vt) = /12 /4 1 

ftiin - g p »r <: i y- iiiayrbej-tead-rly-obtainj erf^f r o m:: e c gi at.ionsi. (2a)^ndr (4}r- _ 

at the required times. 


Geocentric Location of Local Reference Site . - The required spacecraft posi- 
tion vectors relative to the user's local reference frame are obtained 

first in inertial coordinates as (p. - P ) , then transformed to topocentric 

K. S 

coordinates with origin at the local reference site. The true geocentric 

location of the locaL reference site origin is given by the vector p , which 

s 

is calculated from its geographic coordinates il (J) h using the geoid 

S ^ S ) s 

model and rotational transformation respectively illustrated in Figures 4 
and 5. The sketch in Figure 4 represents an x-z meridian plane view of the 
earth in cross-section that shows the relationships between geodetic and 
geocentric latitude and radial distance, which are defined by 





As (p 0 (j)') also define the origin of the local topographic axes , 

S 9 S 9 ■ s 

(U, E, N) , with respect to which the user craft motion is referred, the 
transformation of (p,-p ) to these coordinates is 

K S 



( 8 ) 





where 


I2L 




Pseudorange Measurements , - The remaining step in simulating the pseudo- 
range measurements is to express the in terms of range to the user 
craft, then corrupting the resulting range vectors (R -R) by adding the 

1C • 

simulated GPS receiver bias errors as indicated in Figure 1. This procedure 
is illustrated by the sketch in Figure 6, and the resulting pseudoranges are 
given by 


/ 


cos cos 6 
s s 


-sin-- 5^ — 
s 


i-sin 6’ 
s 


cos 6 


cos <fi 


cos 


*-sin 4)' sin 

1 s 


sin 0- i sin. d 
S| 

I 

I 

0: ' r- a~ 


I 

I 

I 

t cos 




\ + b (9) 

where R is the user's assumed true position in (tJ, E, N) coordinates 
as furnished by a user craft motion simulator sUch as a general aviation 
trainer (GAT). 



Figure 6 . - USER/NAVSTAR Range Geometry. 
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their azimuthal positions and apparent elevations relative to the user 
craft. A much simpler test- is illustrated in Figure 7. The only require- 
ment is to determine whether the spacecraft in question is above the E"-N” 
pianev which- coincides-witlT that -of the-user. craft's wings. Thus, the-k^^ 
spacecraft will not be shielded as long as the Z", component of r -b 



remains positive . 

The apparent spacecraft elevation 



( 10 ) 


( 11 ) 


still must be tested if the receiver antenna coverage is assumed to be 

limited to a minimum value of e, . However, the fact that e, is defined 

k k 

relative to the E"-N" plane, rather than the local horizon (E-N plane), 
still permits avoiding the need to evaluate complicated conditions on the 
spacecraft azimuthal position and the user craft bank angle..- .jwsji.;.; 
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User Craft Position and Bias Error Corrections . - The procedure used for 
obtaining these quantities is based on a linearized Taylor's series expan- 
sion of equation (9) about the current estimate of user craft position and 
GPS receiver bias (see reference 1). This expansion is 




3r, AU 

k- .. + 

9U U 


T T 

where U = [r:^] = [xYZb] and AU = U - U. By expressing equation (9) 

in rectangular form and differentiating, 

3r, AU = 3r, AX + 3r, I AY + 3r AZ + 3r. 1 Ab 
3U U 3X , U 3Y 1 U 3Z U 3b | U 


A . A / A 

X-X^\4X + /y-v^ 


A 

AY + 


Z-Zj^ \ AZ + Ab 


= h. AU 
k 


Rearranging equation (9a) and solving for AU gives 


AU = H ^ Ar 


where Ar = {^1,-^} ^ “ f^l ^2 ^3 estimated 

pseudorange measurements r^^ may be calculated by evaluating equation (9) in 


the form 


A 


A 9 ^2 A 2 ^ 

(X^-X )^ + (\-Y) + ^ 


using the spacecraft ephemeris data and the current estimate of U. 
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User Craft Posltbn and Velocity Update ^, ~ The Mitre GPS navigator 
algorithin is formulated to provide smoothed estimates of updated position 
and velocity, by approximate smoothing backwards in time over the four r^ 
measurements. Tlie procedure is ta calculate AU after each new pseudorange 
measurement is received, by processing it with the most recent values for 
the other three elements of r^^. As the dwell time to receive a pseudorange 

/N - 

meastrrementr is Atr, new values of AU are generated every- 1.2- sec. The— 
corresponding position and velocity updating is accomplished by an a-6 
smoother/predictor of the form 


V(t+At) = V(t) + 6AU/At 


A. /N 


U(t+At) = U(t) + oAU + V (t+At) At 


(13) 


These quantities are also used to estimate cross-track error 


AR^T " OC-X) cos ijt - (Y-Y) sin if; 


(14) 


where 

ip = tan ^ (Vx/Vy) 

in which X, Y, V , and V are outputs from the user craft motion simulator. 
X y 
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CONCLUDING REMARKS : 

The simulator structure described herein provides a useful analytical 
tool for conducting further research and evaluations of navigator algorithms 
based on the use of a low-cost GPS receiver. A simpler test for loss of 
pseudorange measurements due to spacecraft shielding is noted. This test 
eliminates the need for the relatively complex one contained in Mitre’s 
programing (see reference 2). 
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LRCr Implementation of MITRE* s Low-Cost GPS- Navigator^ Simulation 


ci»sHAvtrHPUT.auTPUT*r«pe6.TAPt5-rNPari 
c ir* F% Mooce Fro 

C CP7 PARAMETt*i DEFINITIONS f0« «A0e« FIELD SCEMA»tO ISIMULATEO} 

C CORICIN at act V)A) (ATLANTIC CITY) 

C WITHOUT INTENUOUL OlGPAnATION (JRlAS-0) 

C SET JBlAS-1 FUk INTENTIONAL GPS DEGRADATION 

OINENSION F»(3)#nMECAn>»VT(5)»9X(tfO0)#PY(80O) 

DIMENSION PHASEn).CRAFn3)»tX(4)«NJ$H(A)#0JSH(A) 

OINENSIUN RANG£(<i)#EL(^>#AZ(4)»SATP0S{4t3) 

OIHENSION VECPOwnu VLCIJKt3)#VECUEN(3)#STAC0R(3) 

OITlEi4SI-GN _L^L(2J_ „ . 

CUimaft/NAVEP/LRRQRrA)j_lLAS(Alf EION(A)t£NPf(V» 
CaN?TafY/:tHP«R-/T5T^r^ELrifItFiPlU«- _ 

CQMMON/NVFLG/JhPr, JIONE, JSSEf JCLKE# JBIAS 
COK«ON/NVPAa/ALP-»A»BETA>3bTAl»AOAPTA, ADAPTB 
COrtMON/SATS/JOPb(«iJ» J5ATC A), )ROLO(Al, JSH0<4) 

COHMON/NVEST/USERI A) rVELCA) fJSER S( A) » VELSC A ) 

CALL SEFOMP 

JNPE-1 % JIONE-l S JSSE- 1 S JCLKE-1 S JBIAS-l 
ALPHA-*2 S 9ETA-.01 I BETAl-.Ol % ADAPTA-*2 t AOAPT9-.OT 
TSTRT-13*90 S 0ELT*1*2 S ALT-0* SRLAT«,688629 S PLONG—l* 301608 
J0RQ(l)-2 t JUR3(2)«l S J3R3(3)>1 S JQR8(A)«2 
.JSAT(I)-6 $ JSAT(2)-2 S JSArr3T*A r jrATTAT»7“ 

C INPUT DATA FOR SIMULATED LANDING APPROACH COURSE 
PI»3. 1^X59255358^7'? S RPO-Pl/180. S DPR«180#/PI 
CALL PSEUDO 

00 7 $ 0J$H(JTHJ«O, 

7 JSH0(JTH|»0. 

C SET UP GPS CONSTELLATION PARAMETERS ( A IS IN NAUTICAL HI.) 
A-1.A36826 E^A S 0MEGA(1)«0« S QMEGA(2)«120. S OMEG A ( 3 ) «240« 
FPI1I--15. 1 FP(2)-15* S FP(3)-0» 
ixm«7329 1 IX(2)»16A1 S IX(3)»6753 1 IX(A)«A159 
C INITIALIZE intentional DEGRADATION ERROR BIAS 

ZGAIN«EXP(-A.aoELT/1600. ) 

C TAU • 30 MIN 

CQNG-528«/6080« . 

C SAT 1 SIGMA • 500M20RMS/2*HOOP1*537 

C0NU>2**SaRT(3« )«CONG 
C0NZ«CaNU«S0RT(2»*A.*DELT/ie00») 

C TAU • 30 MIN 

IR(1)«3117 i IR(2) ■ 1379 S IN*1 
00 602 J»1#A 

CALL GETRAN(IR#lN#2«RN#ri#Y2) 

IN - 2 

602 BIASU) • RN « CONG 
C CLOCK ERROR MODEL PARAMETERS 

S0«1000./6030« 

C 1 USEC INITIAL CLOCK OFFSET 

F0»0£LT/6O«6 

C FRACT FREO ERR*06LT*10**9/6050 

F0-0ELT**2/1,216 E^7 

C FRACT FREO ORIFT/S£C*OELTAA2*1O**9/6O0O 

S$«50./60d0* 

FRACT SHORT TERM 5 TABIL ITY*10**9/6080 
SET INTIIAL CRAFT POSITION AND VELOCITY 

DELT»0ELT736OO. S CR AFT( 1 ) -Z/bOaO. 1 CRAFT(2)«X S CRAFT(3)-Y 
00 9 I«l*3 S VEL(I)*0« 

5 US£RCI)-CRAFT(n+ 0 . 

U$ER(A)«VEL(A)-0« 

VEL(1)«Z0T S VEL(2)«X0r S VELC3I-Y0T 
C ITER IS THE ITERATION FOR WHICH PSEUDDRANGE AND 
C ELEVATION OF BEST A SPACECRAFT ARE COMPUTED 

ITER* TIM-0 
C 

C MAIN LOOP STARTS HERE 

33 ITER-ITEP>1 

C KTH IS THE SPACECRAFT NUMBER 

00 17 KTH.1#A 1 NJSH(KTh)»0 S ERPOR(KTH)-0. 

TIM*TIM^1 

C ELAPSED TIME PERIODS (UPDATE INTERVALS) 

TIPE*TSTRT>TIM»06LT 
C TRUE TIME (IN HOURS) 

C SIMULATE TRUE USER CRAFT POSITION AND VELOCITY 
T1M£-(TIME-TSTRT)*3600. 1 0ELT«DELT»3600. 

C CALCULATE TRUE ANOMALY F FOR KTH SPACECRAFT 

F«(FP(JORa(KTHn* ( JSAT(KTH)-1)#A5.^30,*TIME)PRP0 
C CALCULATE SPACECRAFT POW COORDINATES 

C CALCULATE I JK COORDINATES OF KTH SPACECRAFT 
COSNDE«CUS(aMEGA( JOR 3 ( KTH ) ) ♦RPD I 
SINNOE-S IN (OMEGA (JORB( KTH) )♦RPO) 

V£CIJK(l)-A*(CQSlOe*CnS(F)-COS(63.PRPD)PSINNOE*^lN|F)1 
VECI JK(2)-A# (SINN0F*CQS IF )^CUS<63.*RPD}*C0SNDE*SIN(F)) 
VECIJK(3)-A»SIN(63.*RP0)*SIN(F) 

C CONVERT SPACECRAFT COORDINATES FROM IJK TO UEN SYSTEM 


U O O CMJ O O O UOO 
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CALL IJKUHTMP»ALT»RLONGfRLAT»VEClJK»veCU£Nl 
CRAFTin«//oO0O, I C«AFTi2>»X S CRAFT(3l«t 
VVl«VECUtNa)-CRAFT(l) 

VV2«VECU£N(2 l-CF AFT(2) 

VV3«VtCUEN(3)-tR4FU3> 

0EN-VV3 $ IF(*flS(VV3).LT. .0000011 OEN-SIGMC iOOOOOlt VV? 1 
A2CKTH)*ATAN2(VV2»0£N) S IF( AZ(KTH) •IT.O. ) AZ(KTH)«AZC KTH) ♦2,*PI 
OEN.YOT 

C COMPUTE GAT VELOCITY HEADING RELATIVE TO TRUE NORTH COX-OI 
IFtABS(T0T).LT*O J0001) DEN-SIGN ( .000001# YOT ) 

CCRS»ATAN2(<0T#DEN) 

IFCGCRS.LT.O.) GCRS-CCRS^2.PPI 
VVNl-VVl 

VVr«2«VV2*CQS(GCRS)-VV3*SlN(GCRS) 

VVN3»VV3RC0S(GCRS)^VV2^SINCGCRS) 
VVNNl-VVNlRCJS(PHn^VVN2*SIN(PHn 
VVNN2»VVN2*:0S(PHI )-VVNl*SlN(PHII 
VVNN3-VVN3 

RADS ■ ♦ VV3PR2 ’ 

RA0N«SQRT< VVNN2»*2>VVNN3*P2) 

ELIKTH)«ATAM2(VVNN1,RADN1 

C IF SPACECRAFT ELEVATION IS LESS THAN 10 OEG# CHECK FOR SMIFLOXNG 

IFCELIKTH) .LS. CPI/18. n NJSH(KTH)»1 
C 60 FROM SHIELDED TO NOT# OR NOT TO SHIELDED ONLY IF TWO SAME 
C DECISIONS IN SUCCESSION 

lFCNJSH(KTH).E0.3JSH(KTHn JSHOC KTH) -NOSHIKTH) 
OJSH(KTH)«NJSH(KTH) 

IF 4CLKE NOT 0 INCLUDE CLOCK BIAS ERROR 
SS» SHORT TERM STA3ILITY# EOUIV. NAUT. MI. 

SO* STARTING OFFSET# EOUIV. NAUTICAL MI. 

FO* FREQUENCY OFFSET# EOUIV* NAUTICAL MI. 

F0« FREQUENCY DRIFT# EOUIV. NAUTICAL MI. 

IFUCLKE.EQ.O) GOTO A70 
CALL GETRAN(IP#IN#2#RN#Y1#Y2I 
ECB ■ RN ♦ SS 

CBIAS*S0+FQ*TIM*F0*TIM»^24ECB 
ERROR <KTH)»ERRQR(KTH)tC8I AS 
IF JHPE NOT 0 # INCLUDE MULTIPATH ERROR 
470 IFC JMPE.EQ.O) GOTO 410 
ERR0R1*G. 

CALL GETRAN(IR#IN#2#RN»Y1»Y2I 
EMP£(KTH» ■ RN ♦ 35./6080. 

ERROR (KTH)«cRRQR(KTH)+ERR0R1^EMPE(KTH) 

IF JBIAS NOT 0# INCLUDE CORRELATED (30 MIN) NOISE BIAS 
410 IFUBIAS.EO.O) GOTO 460 

CALL GETRANCIR#ISi2#RN#RY#Y2) 
BIAS(KTH)-CaN2A(RY-.5)^ZGAIN*6IASCKTH) 

ERROR (KTH) -ERROR I KTM) 431 AS CKTH) 

IF JIONE NOT 0# INCLUDE IONOSPHERIC DELAY ERROR 
460 IFUIONE.EO.O) GOTO 203 
EPRIM*.947Q8*C0S(ELCKTH) ) 

EION(KTH)-.0052433/SQRT(1.-EPRIMP*2) 
ERROR(KTH)«ERRORiKTH)4EION(KTH) 

203 RANGE ( KTH) -S 3RT ( (VtCUENCD-CRAFTC 1) ) **2^RA0S )^EPROR{KTH) 

00 204 I*l#3 

204 SATPOS(KTH#n-VECUENCI) 

IFCITER.EO.I.ANO.KTH.LT.4) GOTO 17 

DO ESTIMATE OF USER PREDICTED POSITION (USER) AND VELOCITY (VEU 
AND OF SMOOTHED USER POSITION (USERS) AND VELOCITY (VELS) 

CALL ESTIM(RANGE#SATPOS#USER#VEL#US£RS#VELS»KTH) 

C COMPUTE CROSSTRACK NAVIGATION ERROR 

XX-USEa(2)-CRAFTC2) $ YY-U5ER(3)-CRAFT(3) 

CTE-XXACOS (OCRS )-YY*SIN( OCRS) 

ATE - XX ♦ SINCGCRSI ♦ Yt ♦ COSClGCRS) 

ENV • S0RT(XX**2 ♦ fY**2) 

ELEV-EL(KTH)*OPR » AZIM-AZ(KTH) PDPR 
HDG-PSIPOPR S BANK-PHl«DPR 
17 CONTINUE 

C THIS IS THE END OF ONE ITERATION 

- GOTO 33 
STOP 
END 
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5UBROUTIN6 eSTIH(RANr.f#SATPDS#USE«#VEL#USERS»VflS#KTHJ 
OIPENSION RANGFU) •USERS(<r).VEL.^C4) •VEL(4l#SATPn^(4#l),iJSERUI, 
lHMAT(<»#4)»R-»AR(^».nELR(4)»OELU(4i),BB<4#l),IPlVOT{4)rIHn?t««>, 
2HR«ATt3#3) 

COrlhON/TMPAR/TSTRT.OEtT.ITER»PSIH 
CDMMQn/NVFLG/J«PE»JIQNE,JSSE» JCLK6#JRUS 
COJ<HON/NVPAR/ALPHA^d£TA#^BETAl#AOAPTA>ADAPTB- ^ 

CO«Ka:i/:kArsy jaRBt<a»4SA.TCA>»OROLO(^lj^SHat4I 

iJ^cxrtr;jn^xirGxiniiia= 

00 11 J-l»^ S DELRf Jl • Oi 
11 6BU#l)-0, 

10 CONTINUE 

C USER ESTIMATE OF USER POSITION 
C HMAT M MATRIX 

RBARCKTH)«(SATPOS(KTH,l»-USERa ))P*2^(SATP0S(KTHf 2I-USFRC2) M»2 
RBAR|KTH)«SORT<RRAPCKTH)+(SATPOS(KTHf 3I-USERC3))A*2) 
0ELR(KTH)»RANGE(<TH)-R3ARUTH»-USERCAI 
C SIMU LATE SHIELDIN G IP JSSE NOT 0 

rF f «l S S E • E~3 • 0T~ GTO TO 213 

C IF SHIELDING SIMULATED# FIND FIRST SPACECRAFT SMIELOEO IF ANY 
00 211 J-l#4 

IFUSHOI J).NE.O) GOTO 210 
211 CONTINUE 

GOTO 213 * 

C NO SPACECRAFT SHIELDED 

210 NS-J 

C NUMBER OF FIRST SPACECRAFT SHIELDED 

C FORM 3X3 HRMAT FROM VISIBLE SPACECRAFT 

K-0 

00 215 J-l#3 
K«K+l 

IFCK.EO.NS) K-K^l 
C SKIP SHIELDED SPACECRAFT 

DO 215 JCQL-X>3 

215 HRMAT CJ# JCOD-tUSER UC0U-SATP0S(K# JCOLM/(RANGE(K)-USER(An 
CALL MATINVI 3f 3»HRMATf I#39tO*OET#XSCALE*IPlVOT#TNOEX) 

C COAST CLOCK BIAS DURING SHIELDING 

DO 216 I«l#<* 

216 DELU(t)*0, 

00 217 I«I#3 
K«0 

DO 217 J-l#3 
K-K^l 

IF(K*EO«NS) K-K^l 

C SKIP SMIELOEO SPACECRAFT 

217 OELU( n >DELU ( I) >HRMAT(I> J )*OELR (K) 

GOTO 33 

C CALCULATE H MATRIX FOR ALL FOUR SPACECRAFT 
213 DO 2^ 

2^ HMATIJ»^I«1. 

DO 25 J-l»^ 

DO 25 JC0L«1»3 

HMATI J# JC3U«CUSER(JCOL)-SATPOS(J»JCOLU/(RANGECJ)-USFRe4)) 

25 CONTINUE 

CALL MATINVt4,4#HMAT # 1» 83#0# DET# ISC ALE# IPIVOT# INDEX) 

C CALCULATE OELTA-U 3Y MATRIX MULTIPLY 

00 34 I«l#4 
34 OELUm«0. 

00 26 I-l#4 
00 26 K-l»4 

26 DELU{I)«HMATCIfK)POELR(K)^DELUm 

C UPDATE USER ESTIMATE BT ALPHA-BETA TRACKER 
C SMOOTHING AND PREDICTION BY ALPHA-BEtA 

33 DO 61 J»l#4 . 

USERSIJ)«USER(J)*ALPHAP0ELU(J) 

IFU.NE.U GOTO 64 

VELS CJ)»VEL( J)^.2*8ETAP0ELU( Jl/OELT 

GOTO 65 

64 VELSU)«VcL< J)^8ETA*0ELU« J)/OELT 

65 VELCJ)-V£LSC J) 

61 US£R(J)»USERSCJ)fOELT*VELSU) 

RETURN 

END 
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SUBROUTINE IJKUEC TIME, 4LT»RIGNG#RL AT* VECXJ*C,VECUeN) 

DIMENSION V£CIJK(3),VECUEN(3J#VEC(3)#STACORf3)#T»IJK(3f3) 

C alt • station altitude of U-c-N system* in nautical hi, 

C RLONG - STATION LONGITUDE OF U-E-N SYSTEM 9 IN RADIANS 

C RLAT • STATION LATITUDE OF U-E-N SYSTEM * IN RADIANS 

C IJKUE IS A SUBROUTINE FOR COORDINATE TRANS FORMATinN «ETWEEN 

C I-J-K (GEOCENTRIC EOUATORIALI AND U-E-N ITOPOCENTdC LCCALI 

C COORDINATE FRAMES 

C 

c 

C 0.2618 IS EARTH TURN RATE (15 DEG/HR) IN RAO/HR UNITS 

THR».26l6ATlM£tRL0NG $ 5 INTH-S IN( THR ) 1 COSTH-COS (THR ) 

SINPHI-SIN(RLAT) i COSRHi-COS ( RL AT) 

C COMPUTE THE STATION COORDINATES OF THE U-E-N SYSTEM ORIGIN IN 
C I-J-K COORDINATE FRAME 

ECCSQ-l.-.^J^bbAS^^Z S OENO«SOPT(1.-ECCSQ»SINPHI*^2) 

X *(3443. 936/ OENO>ALT)*COS PHI 

2«(3A<i3.936^(1.-sCCSQ)/OENO^ALT)*SINPHI 

RH0»SQRT(X**2^2**2) 

SINPHI-Z/RHO $ COSPHI-X/RHO 

STACQRdJ-X^COSTH S ST ACOR ( 2) "X^S INTH S STAC0R(3)»2 
C COMPUTE THE TRANSFORMATION MATRIX FOR I-J-K TO U-E-N SYSTEMS 
C 

TRIJK(1#1)»CQSPHI*C0STH 
TRIJK(1*2) -COSPHI^SINTH 
TRIJK(1*3)-SINPHI 

TRIJK(2*1)*-SINTH S TR I JK( 2*2) -COSTH $ TRI JK(2#3)»0. 

TRIJK(3*1)—SINPHI*C0STH 
TRIJK(3*2)— SINPHI*SINTH 
TRIJK(3*3)-C0SPHI 

C COMPUTE TRANSFORMATION FROM I-J-K TO U-E-N FRAMES 
DO 22 I«l#3 

22 VECCI)*VECIJK(I)-STACOR(I) 

C VEC STORES THE POSITION COORDINATES OF THE SPACECRAFT W.R.T. TO 

C THE U-E-N ORIGIN* BUT IN I-J-K COORDINATES 
DO 23 I«l#3 
VECUEN(I)«0. 

DO 23 J«l*3 ^ 

23 VECUEN(1)«VECUEN(1)^TRI JK(1*J)«VEC( J) 

RETURN 

END 
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